%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 《控制之美-卷二》 代码
%% 作者：王天威，黄军魁
%% 清华大学出版社
%% 程序名称：LQR_InvertedPed_Pendulum
%% 程序功能：平衡车控制 - 连续系统案例分析 （4.4.5节案例）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% 程序初始化，清空工作空间，缓存，
clear all;
close all;
clc;
% 读取Octave控制数据库（注：如使用Matlab，可删除或注释掉本行代码）
pkg load control;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 定义系统参数
% 定义重力加速度
g = 10;
% 定义平衡车连杆长度
d = 1;
%%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%%
% 定义系统矩阵A
A = [0 1; g/d 0];
% 计算系统矩阵A的维度
n= size (A,1);
% 定义输入矩阵B
B = [0; 1];
% 计算输入矩阵B的维度
p = size(B,2);
% 定义矩阵C
C = [1, 0];
% 定义矩阵D
D = 0;
%%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%%
% 设计系统状态权重矩阵q1，维度n x n,案例1
q1 = [100 0;0 1];
% 设计系统状态权重矩阵q2，维度n x n,案例2
q2 = [1 0;0 100];
% 设计系统状态权重矩阵q3，维度n x n,案例3
q3 = [1 0;0 1];
% 设计系统输入权重矩阵r1，维度p x p，案例1
r1 = 1;
% 设计系统输入权重矩阵r2，维度p x p，案例2
r2 = 1;
% 设计系统输入权重矩阵r3，维度p x p，案例3
r3 = 1;
%%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%%
% 状态初始化
x0 = [pi/20;0]; x = x0;

%%%%%%%%%%%%%代数Riccati方程求解%%%%%%%%%%%%%%%%
% Riccati方程求解，案例1
[P1,L1,G1] = care(A,B,q1,r1);
% 计算反馈增益，参考式（4.4.52）
K1=inverse(r1)*B'*P1;

% Riccati方程求解，案例2
[P2,L2,G2] = care(A,B,q2,r2);
% 计算反馈增益，参考式（4.4.52）
K2=inverse(r2)*B'*P2;

% Riccati方程求解，案例3
[P3,L3,G3] = care(A,B,q3,r3);
% 计算反馈增益，参考式（4.4.52）
K3=inverse(r3)*B'*P3;

%%%%%%%%%%%%%%%构建闭环系统%%%%%%%%%%%%%%%%%%%%%
% 构建闭环状态空间方程，案例1
sys_cl1=ss(A-B*K1,[0;0],C,D);
% 构建闭环状态空间方程，案例2
sys_cl2=ss(A-B*K2,[0;0],C,D);
% 构建闭环状态空间方程，案例3
sys_cl3=ss(A-B*K3,[0;0],C,D);

%%%%%%%%%%%%%%%系统仿真开始%%%%%%%%%%%%%%%%%%%%%
t_span = 0.01;
% 系统时间离散
t=0:t_span:5;
% 闭环系统初值响应，案例1
[y1,t,x1]=initial(sys_cl1,x,t);
% 闭环系统初值响应，案例2
[y2,t,x2]=initial(sys_cl2,x,t);
% 闭环系统初值响应，案例3
[y3,t,x3]=initial(sys_cl3,x,t);

%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1, 'position',[150 50 800 950]);
%% 系统响应视图，状态量x1（角度）
subplot(3,1,1);
% 测试1结果
plot(t,x1(:,1),"linewidth",2);
% 近似计算x1的积分
x1_1_cost = x1(:,1)'*x1(:,1)*t_span;
hold on;
% 测试2结果
plot(t,x2(:,1),'--',"linewidth",2);
% 近似计算x1的积分
x2_1_cost = x2(:,1)'*x2(:,1)*t_span;
hold on;
% 测试3结果
plot(t,x3(:,1),'-.',"linewidth",2);
% 近似计算x1的积分
x3_1_cost = x3(:,1)'*x3(:,1)*t_span;
legend('测试一','测试二','测试三',"FontSize",14);
xlim([0 4]);
grid on;
hold off;

%% 系统响应结果视图，状态量x2（角速度）
subplot(3,1,2);
% 测试1结果
plot(t,x1(:,2),"linewidth",2);
% 近似计算x2的积分
x1_2_cost = x1(:,2)'*x1(:,2)*t_span;
hold on;
% 测试2结果
plot(t,x2(:,2),'--',"linewidth",2);
% 近似计算x2的积分
x2_2_cost = x2(:,2)'*x2(:,2)*t_span;
hold on;
% 测试3结果
plot(t,x3(:,2),'-.',"linewidth",2);
% 近似计算x2的积分
x3_2_cost = x3(:,2)'*x3(:,2)*t_span;
legend('测试一','测试二','测试三',"FontSize",14);
xlim([0 4]);
grid on;
hold off;


%% 系统响应结果视图，输入（加速度）
subplot(3,1,3);
% 测试1结果
plot(t,-K1*x1',"linewidth",2);
% 近似计算u的总代价
u1_cost = (-K1*x1')*(-K1*x1')'*t_span';
hold on;
% 测试2结果
plot(t,-K2*x2','--',"linewidth",2);
% 近似计算u的总代价
u2_cost = (-K2*x2')*(-K2*x2')'*t_span;
hold on;
% 测试3结果
plot(t,-K3*x3','-.',"linewidth",2);
% 近似计算u的总代价
u3_cost = (-K3*x3')*(-K3*x3')'*t_span;
legend('测试一','测试二','测试三',"FontSize",14);
xlim([0 4]);
grid on;
hold off;
